// Copyright 2008 Isis Innovation Limited
#include "RelocaliserSBI.h"
#include "SmallBlurryImage.h"
#include <cvd/utility.h>
#include <gvars3/instances.h>

using namespace CVD;
using namespace std;
using namespace GVars3;

RelocaliserSBI::RelocaliserSBI(const ATANCamera &camera)
  : mCamera(camera)
{
};

SE3<> RelocaliserSBI::BestPose()
{
  return mse3Best;
}

bool RelocaliserSBI::AttemptRecovery(Map *pMap, boost::shared_ptr<KeyFrame> pCurrentKF, boost::shared_ptr<KeyFrame>  pMapKF)
{
  struct timeval start, end;
  gettimeofday(&start, NULL);

  // Ensure the incoming frame has a SmallBlurryImage attached
  if(!pCurrentKF->pSBI)
    pCurrentKF->pSBI = new SmallBlurryImage(*pCurrentKF);
  else
    pCurrentKF->pSBI->MakeFromKF(*pCurrentKF);
  
  // Find the best ZMSSD match from all keyframes in map
  boost::shared_ptr<KeyFrame> pBestMatchKF = ScoreKFs(pMap, pCurrentKF, pMapKF);

  if(pBestMatchKF != boost::shared_ptr<KeyFrame>())
  {
    // And estimate a camera rotation from a 3DOF image alignment
    pair<SE2<>, double> result_pair = pCurrentKF->pSBI->IteratePosRelToTarget(*(pBestMatchKF->pSBI), 6);
    mse2 = result_pair.first;
    double dScore =result_pair.second;
    
    SE3<> se3KeyFramePos = pBestMatchKF->se3CfromW;
    mse3Best = SmallBlurryImage::SE3fromSE2(mse2, mCamera) * se3KeyFramePos;
    
    if(dScore < GV2.GetDouble("Reloc2.MaxScore", 9e6, SILENT))
    {
      gettimeofday(&end, NULL);
      long mtime = ((end.tv_sec - start.tv_sec)*1000 + (end.tv_usec - start.tv_usec)/1000.0) + 0.5;
      std::cout << "SBI: " << mtime << " milliseconds - " << std::endl;

      return true;
    }
    else 
      return false;
  }
  else
    return false;
};

// Compare current KF to closests KFs in map by Zero-mean SSD
boost::shared_ptr<KeyFrame>  RelocaliserSBI::ScoreKFs(Map *pMap, boost::shared_ptr<KeyFrame> pCurrentKF, boost::shared_ptr<KeyFrame> pMapKF)
{
  static gvar3<int> gvnMaxSearchKeyFrames("Relocaliser.MaxSearchKeyFrames", 100, SILENT);

  mdBestScore = 99999999999999.9;
  boost::shared_ptr<KeyFrame> pBestKF = boost::shared_ptr<KeyFrame>();

  //Grab the maximum number of keyframes we can handle (sorted by their proximity to most recent keyframe)
  Vector<3> v3MostRecentPos = pMapKF->se3CfromW.inverse().get_translation();
  std::vector<std::pair<double, boost::shared_ptr<KeyFrame> > > vKeyFrames;
  pMap->mrtAllKeyFrames.NearestNeighbours(v3MostRecentPos.get_data_ptr(), pMapKF, *gvnMaxSearchKeyFrames, vKeyFrames, 2);

  for(unsigned int i = 0; i < vKeyFrames.size(); ++i)
  {
    double dSSD = pCurrentKF->pSBI->ZMSSD(*(vKeyFrames[i].second->pSBI));
    if(dSSD < mdBestScore)
	  {
	    mdBestScore = dSSD;
	    pBestKF = vKeyFrames[i].second;
	  }    
  }

  return pBestKF;
}

